/****************************************************************************
 * apps/examples/nimble_blecent/peer.c
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.  The
 * ASF licenses this file to you under the Apache License, Version 2.0 (the
 * "License"); you may not use this file except in compliance with the
 * License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
 * License for the specific language governing permissions and limitations
 * under the License.
 *
 ****************************************************************************/

/****************************************************************************
 * Included Files
 ****************************************************************************/

#include <assert.h>
#include <string.h>
#include "host/ble_hs.h"
#include "blecent.h"

/****************************************************************************
 * Private Function Prototypes
 ****************************************************************************/

FAR static struct peer_svc *
peer_svc_find_range(FAR struct peer *peer, uint16_t attr_handle);
FAR static struct peer_svc *
peer_svc_find(FAR struct peer *peer, uint16_t svc_start_handle,
              struct peer_svc **out_prev);
static int peer_svc_is_empty(const struct peer_svc *svc);

static uint16_t chr_end_handle(FAR const struct peer_svc *svc,
                               FAR const struct peer_chr *chr);
static int chr_is_empty(FAR const struct peer_svc *svc,
                        FAR const struct peer_chr *chr);
FAR static struct peer_chr *
peer_chr_find(FAR const struct peer_svc *svc, uint16_t chr_def_handle,
              FAR struct peer_chr **out_prev);
static void peer_disc_chrs(FAR struct peer *peer);

static int
peer_dsc_disced(uint16_t conn_handle, FAR const struct ble_gatt_error *error,
                uint16_t chr_val_handle, FAR const struct ble_gatt_dsc *dsc,
                FAR void *arg);

/****************************************************************************
 * Private Data
 ****************************************************************************/

FAR static void *peer_svc_mem;
static struct os_mempool peer_svc_pool;

FAR static void *peer_chr_mem;
static struct os_mempool peer_chr_pool;

FAR static void *peer_dsc_mem;
static struct os_mempool peer_dsc_pool;

FAR static void *peer_mem;
static struct os_mempool peer_pool;
static SLIST_HEAD(, peer) peers;

/****************************************************************************
 * Public Functions
 ****************************************************************************/

FAR static struct peer *peer_find(uint16_t conn_handle)
{
  struct peer *peer;

  SLIST_FOREACH(peer, &peers, next)
    {
      if (peer->conn_handle == conn_handle)
        {
          return peer;
        }
    }

  return NULL;
}

static void peer_disc_complete(FAR struct peer *peer, int rc)
{
  peer->disc_prev_chr_val = 0;

  /* Notify caller that discovery has completed. */

  if (peer->disc_cb != NULL)
    {
      peer->disc_cb(peer, rc, peer->disc_cb_arg);
    }
}

FAR static struct peer_dsc *
peer_dsc_find_prev(FAR const struct peer_chr *chr, uint16_t dsc_handle)
{
  FAR struct peer_dsc *prev;
  FAR struct peer_dsc *dsc;

  prev = NULL;
  SLIST_FOREACH(dsc, &chr->dscs, next)
    {
      if (dsc->dsc.handle >= dsc_handle)
        {
          break;
        }

      prev = dsc;
    }

  return prev;
}

FAR static struct peer_dsc *
peer_dsc_find(FAR const struct peer_chr *chr, uint16_t dsc_handle,
              FAR struct peer_dsc **out_prev)
{
  FAR struct peer_dsc *prev;
  FAR struct peer_dsc *dsc;

  prev = peer_dsc_find_prev(chr, dsc_handle);
  if (prev == NULL)
    {
      dsc = SLIST_FIRST(&chr->dscs);
    }
  else
    {
      dsc = SLIST_NEXT(prev, next);
    }

  if (dsc != NULL && dsc->dsc.handle != dsc_handle)
    {
      dsc = NULL;
    }

  if (out_prev != NULL)
    {
      *out_prev = prev;
    }

  return dsc;
}

static int
peer_dsc_add(FAR struct peer *peer, uint16_t chr_val_handle,
             FAR const struct ble_gatt_dsc *gatt_dsc)
{
  FAR struct peer_dsc *prev;
  FAR struct peer_dsc *dsc;
  FAR struct peer_svc *svc;
  FAR struct peer_chr *chr;

  svc = peer_svc_find_range(peer, chr_val_handle);
  if (svc == NULL)
    {
      /* Can't find service for discovered descriptor;
       * this shouldn't happen.
       */

      assert(0);
      return BLE_HS_EUNKNOWN;
    }

  chr = peer_chr_find(svc, chr_val_handle, NULL);
  if (chr == NULL)
    {
      /* Can't find characteristic for discovered descriptor;
       * this shouldn't happen.
       */

      assert(0);
      return BLE_HS_EUNKNOWN;
    }

  dsc = peer_dsc_find(chr, gatt_dsc->handle, &prev);
  if (dsc != NULL)
    {
      /* Descriptor already discovered. */

      return 0;
    }

  dsc = os_memblock_get(&peer_dsc_pool);
  if (dsc == NULL)
    {
      /* Out of memory. */

      return BLE_HS_ENOMEM;
    }

  memset(dsc, 0, sizeof *dsc);

  dsc->dsc = *gatt_dsc;

  if (prev == NULL)
    {
      SLIST_INSERT_HEAD(&chr->dscs, dsc, next);
    }
  else
    {
      SLIST_NEXT(prev, next) = dsc;
    }

  return 0;
}

static void peer_disc_dscs(FAR struct peer *peer)
{
  FAR struct peer_chr *chr;
  FAR struct peer_svc *svc;
  int rc;

  /* Search through the list of discovered characteristics for the first
   * characteristic that contains undiscovered descriptors.
   * Then, discover all descriptors belonging to that characteristic.
   */

  SLIST_FOREACH(svc, &peer->svcs, next)
    {
      SLIST_FOREACH(chr, &svc->chrs, next)
        {
          if (!chr_is_empty(svc, chr) &&
              SLIST_EMPTY(&chr->dscs) &&
              peer->disc_prev_chr_val <= chr->chr.def_handle)
            {
              rc = ble_gattc_disc_all_dscs(peer->conn_handle,
                                          chr->chr.val_handle,
                                          chr_end_handle(svc, chr),
                                          peer_dsc_disced, peer);
              if (rc != 0)
                {
                  peer_disc_complete(peer, rc);
                }

              peer->disc_prev_chr_val = chr->chr.val_handle;
              return;
            }
        }
    }

  /* All descriptors discovered. */

  peer_disc_complete(peer, 0);
}

static int
peer_dsc_disced(uint16_t conn_handle, FAR const struct ble_gatt_error *error,
                uint16_t chr_val_handle, FAR const struct ble_gatt_dsc *dsc,
                FAR void *arg)
{
  FAR struct peer *peer;
  int rc;

  peer = arg;
  assert(peer->conn_handle == conn_handle);

  switch (error->status)
    {
      case 0:
        rc = peer_dsc_add(peer, chr_val_handle, dsc);
        break;

      case BLE_HS_EDONE:
        /* All descriptors in this characteristic discovered;
         * start discovering descriptors in the next characteristic.
         */

        if (peer->disc_prev_chr_val > 0)
          {
            peer_disc_dscs(peer);
          }

        rc = 0;
        break;

      default:

        /* Error; abort discovery. */

        rc = error->status;
        break;
    }

  if (rc != 0)
    {
      /* Error; abort discovery. */

      peer_disc_complete(peer, rc);
    }

  return rc;
}

static uint16_t chr_end_handle(FAR const struct peer_svc *svc,
                               FAR const struct peer_chr *chr)
{
  FAR const struct peer_chr *next_chr;

  next_chr = SLIST_NEXT(chr, next);
  if (next_chr != NULL)
    {
      return next_chr->chr.def_handle - 1;
    }
  else
    {
      return svc->svc.end_handle;
    }
}

static int chr_is_empty(FAR const struct peer_svc *svc,
                        FAR const struct peer_chr *chr)
{
  return chr_end_handle(svc, chr) <= chr->chr.val_handle;
}

FAR static struct peer_chr *
peer_chr_find_prev(const struct peer_svc *svc, uint16_t chr_val_handle)
{
  FAR struct peer_chr *prev;
  FAR struct peer_chr *chr;

  prev = NULL;
  SLIST_FOREACH(chr, &svc->chrs, next)
    {
      if (chr->chr.val_handle >= chr_val_handle)
        {
          break;
        }

      prev = chr;
    }

  return prev;
}

FAR static struct peer_chr *
peer_chr_find(FAR const struct peer_svc *svc, uint16_t chr_val_handle,
              FAR struct peer_chr **out_prev)
{
  FAR struct peer_chr *prev;
  FAR struct peer_chr *chr;

  prev = peer_chr_find_prev(svc, chr_val_handle);
  if (prev == NULL)
    {
      chr = SLIST_FIRST(&svc->chrs);
    }
  else
    {
      chr = SLIST_NEXT(prev, next);
    }

  if (chr != NULL && chr->chr.val_handle != chr_val_handle)
    {
      chr = NULL;
    }

  if (out_prev != NULL)
    {
      *out_prev = prev;
    }

  return chr;
}

static void peer_chr_delete(FAR struct peer_chr *chr)
{
  FAR struct peer_dsc *dsc;

  while ((dsc = SLIST_FIRST(&chr->dscs)) != NULL)
    {
      SLIST_REMOVE_HEAD(&chr->dscs, next);
      os_memblock_put(&peer_dsc_pool, dsc);
    }

  os_memblock_put(&peer_chr_pool, chr);
}

static int
peer_chr_add(FAR struct peer *peer, uint16_t svc_start_handle,
             FAR const struct ble_gatt_chr *gatt_chr)
{
  FAR struct peer_chr *prev;
  FAR struct peer_chr *chr;
  FAR struct peer_svc *svc;

  svc = peer_svc_find(peer, svc_start_handle, NULL);
  if (svc == NULL)
    {
      /* Can't find service for discovered characteristic; this shouldn't
       * happen.
       */

      assert(0);
      return BLE_HS_EUNKNOWN;
    }

  chr = peer_chr_find(svc, gatt_chr->def_handle, &prev);
  if (chr != NULL)
    {
      /* Characteristic already discovered. */

      return 0;
    }

  chr = os_memblock_get(&peer_chr_pool);
  if (chr == NULL)
    {
      /* Out of memory. */

      return BLE_HS_ENOMEM;
    }

  memset(chr, 0, sizeof *chr);

  chr->chr = *gatt_chr;

  if (prev == NULL)
    {
      SLIST_INSERT_HEAD(&svc->chrs, chr, next);
    }
  else
    {
      SLIST_NEXT(prev, next) = chr;
    }

  return 0;
}

static int
peer_chr_disced(uint16_t conn_handle, FAR const struct ble_gatt_error *error,
                FAR const struct ble_gatt_chr *chr, FAR void *arg)
{
  FAR struct peer *peer;
  int rc;

  peer = arg;
  assert(peer->conn_handle == conn_handle);

  switch (error->status)
    {
      case 0:
        rc = peer_chr_add(peer, peer->cur_svc->svc.start_handle, chr);
        break;

      case BLE_HS_EDONE:
        /* All characteristics in this service discovered; start discovering
         * characteristics in the next service.
         */

        if (peer->disc_prev_chr_val > 0)
          {
            peer_disc_chrs(peer);
          }

        rc = 0;
        break;

      default:
        rc = error->status;
        break;
    }

  if (rc != 0)
    {
      /* Error; abort discovery. */

      peer_disc_complete(peer, rc);
    }

  return rc;
}

static void peer_disc_chrs(FAR struct peer *peer)
{
  FAR struct peer_svc *svc;
  int rc;

  /* Search through the list of discovered service for the first
   * service that contains undiscovered characteristics.
   * Then, discover all characteristics belonging to that service.
   */

  SLIST_FOREACH(svc, &peer->svcs, next)
    {
      if (!peer_svc_is_empty(svc) && SLIST_EMPTY(&svc->chrs))
        {
          peer->cur_svc = svc;
          rc = ble_gattc_disc_all_chrs(peer->conn_handle,
                                       svc->svc.start_handle,
                                       svc->svc.end_handle,
                                       peer_chr_disced, peer);
          if (rc != 0)
            {
              peer_disc_complete(peer, rc);
            }

          return;
        }
    }

  /* All characteristics discovered. */

  peer_disc_dscs(peer);
}

static int peer_svc_is_empty(FAR const struct peer_svc *svc)
{
  return svc->svc.end_handle <= svc->svc.start_handle;
}

FAR static struct peer_svc *
peer_svc_find_prev(FAR struct peer *peer, uint16_t svc_start_handle)
{
  FAR struct peer_svc *prev;
  FAR struct peer_svc *svc;

  prev = NULL;
  SLIST_FOREACH(svc, &peer->svcs, next)
    {
      if (svc->svc.start_handle >= svc_start_handle)
        {
          break;
        }

      prev = svc;
    }

  return prev;
}

FAR static struct peer_svc *
peer_svc_find(FAR struct peer *peer, uint16_t svc_start_handle,
              FAR struct peer_svc **out_prev)
{
  FAR struct peer_svc *prev;
  FAR struct peer_svc *svc;

  prev = peer_svc_find_prev(peer, svc_start_handle);
  if (prev == NULL)
    {
      svc = SLIST_FIRST(&peer->svcs);
    }
  else
    {
      svc = SLIST_NEXT(prev, next);
    }

  if (svc != NULL && svc->svc.start_handle != svc_start_handle)
    {
      svc = NULL;
    }

  if (out_prev != NULL)
    {
      *out_prev = prev;
    }

  return svc;
}

FAR static struct peer_svc *
peer_svc_find_range(FAR struct peer *peer, uint16_t attr_handle)
{
  FAR struct peer_svc *svc;

  SLIST_FOREACH(svc, &peer->svcs, next)
    {
      if (svc->svc.start_handle <= attr_handle &&
          svc->svc.end_handle >= attr_handle)
        {
          return svc;
        }
    }

  return NULL;
}

FAR const struct peer_svc *
peer_svc_find_uuid(FAR const struct peer *peer, FAR const ble_uuid_t *uuid)
{
  FAR const struct peer_svc *svc;

  SLIST_FOREACH(svc, &peer->svcs, next)
    {
      if (ble_uuid_cmp(&svc->svc.uuid.u, uuid) == 0)
        {
          return svc;
        }
    }

  return NULL;
}

FAR const struct peer_chr *
peer_chr_find_uuid(FAR const struct peer *peer,
                   FAR const ble_uuid_t *svc_uuid,
                   FAR const ble_uuid_t *chr_uuid)
{
  FAR const struct peer_svc *svc;
  FAR const struct peer_chr *chr;

  svc = peer_svc_find_uuid(peer, svc_uuid);
  if (svc == NULL)
    {
      return NULL;
    }

  SLIST_FOREACH(chr, &svc->chrs, next)
    {
      if (ble_uuid_cmp(&chr->chr.uuid.u, chr_uuid) == 0)
        {
          return chr;
        }
    }

  return NULL;
}

FAR const struct peer_dsc *
peer_dsc_find_uuid(FAR const struct peer *peer,
                   FAR const ble_uuid_t *svc_uuid,
                   FAR const ble_uuid_t *chr_uuid,
                   FAR const ble_uuid_t *dsc_uuid)
{
  FAR const struct peer_chr *chr;
  FAR const struct peer_dsc *dsc;

  chr = peer_chr_find_uuid(peer, svc_uuid, chr_uuid);
  if (chr == NULL)
    {
      return NULL;
    }

  SLIST_FOREACH(dsc, &chr->dscs, next)
    {
      if (ble_uuid_cmp(&dsc->dsc.uuid.u, dsc_uuid) == 0)
        {
          return dsc;
        }
    }

  return NULL;
}

static int
peer_svc_add(FAR struct peer *peer, FAR const struct ble_gatt_svc *gatt_svc)
{
  FAR struct peer_svc *prev;
  FAR struct peer_svc *svc;

  svc = peer_svc_find(peer, gatt_svc->start_handle, &prev);
  if (svc != NULL)
    {
      /* Service already discovered. */

      return 0;
    }

  svc = os_memblock_get(&peer_svc_pool);
  if (svc == NULL)
    {
      /* Out of memory. */

      return BLE_HS_ENOMEM;
    }

  memset(svc, 0, sizeof *svc);

  svc->svc = *gatt_svc;
  SLIST_INIT(&svc->chrs);

  if (prev == NULL)
    {
      SLIST_INSERT_HEAD(&peer->svcs, svc, next);
    }
  else
    {
      SLIST_INSERT_AFTER(prev, svc, next);
    }

  return 0;
}

static void peer_svc_delete(FAR struct peer_svc *svc)
{
  FAR struct peer_chr *chr;

  while ((chr = SLIST_FIRST(&svc->chrs)) != NULL)
    {
      SLIST_REMOVE_HEAD(&svc->chrs, next);
      peer_chr_delete(chr);
    }

  os_memblock_put(&peer_svc_pool, svc);
}

static int
peer_svc_disced(uint16_t conn_handle, FAR const struct ble_gatt_error *error,
                FAR const struct ble_gatt_svc *service, FAR void *arg)
{
  FAR struct peer *peer;
  int rc;

  peer = arg;
  assert(peer->conn_handle == conn_handle);

  switch (error->status)
    {
      case 0:
        rc = peer_svc_add(peer, service);
        break;

      case BLE_HS_EDONE:

        /* All services discovered; start discovering characteristics. */

        if (peer->disc_prev_chr_val > 0)
          {
            peer_disc_chrs(peer);
          }

        rc = 0;
        break;

      default:
        rc = error->status;
        break;
    }

  if (rc != 0)
    {
      /* Error; abort discovery. */

      peer_disc_complete(peer, rc);
    }

  return rc;
}

int peer_disc_all(uint16_t conn_handle, FAR peer_disc_fn *disc_cb,
                  FAR void *disc_cb_arg)
{
  FAR struct peer_svc *svc;
  FAR struct peer *peer;
  int rc;

  peer = peer_find(conn_handle);
  if (peer == NULL)
    {
      return BLE_HS_ENOTCONN;
    }

  /* Undiscover everything first. */

  while ((svc = SLIST_FIRST(&peer->svcs)) != NULL)
    {
      SLIST_REMOVE_HEAD(&peer->svcs, next);
      peer_svc_delete(svc);
    }

  peer->disc_prev_chr_val = 1;
  peer->disc_cb           = disc_cb;
  peer->disc_cb_arg       = disc_cb_arg;

  rc = ble_gattc_disc_all_svcs(conn_handle, peer_svc_disced, peer);
  if (rc != 0)
    {
      return rc;
    }

  return 0;
}

int peer_delete(uint16_t conn_handle)
{
  FAR struct peer_svc *svc;
  FAR struct peer *peer;
  int rc;

  peer = peer_find(conn_handle);
  if (peer == NULL)
    {
      return BLE_HS_ENOTCONN;
    }

  SLIST_REMOVE(&peers, peer, peer, next);

  while ((svc = SLIST_FIRST(&peer->svcs)) != NULL)
    {
      SLIST_REMOVE_HEAD(&peer->svcs, next);
      peer_svc_delete(svc);
    }

  rc = os_memblock_put(&peer_pool, peer);
  if (rc != 0)
    {
      return BLE_HS_EOS;
    }

  return 0;
}

int peer_add(uint16_t conn_handle)
{
  FAR struct peer *peer;

  /* Make sure the connection handle is unique. */

  peer = peer_find(conn_handle);
  if (peer != NULL)
    {
      return BLE_HS_EALREADY;
    }

  peer = os_memblock_get(&peer_pool);
  if (peer == NULL)
    {
      /* Out of memory. */

      return BLE_HS_ENOMEM;
    }

  memset(peer, 0, sizeof *peer);
  peer->conn_handle = conn_handle;

  SLIST_INSERT_HEAD(&peers, peer, next);

  return 0;
}

static void peer_free_mem(void)
{
  free(peer_mem);
  peer_mem = NULL;

  free(peer_svc_mem);
  peer_svc_mem = NULL;

  free(peer_chr_mem);
  peer_chr_mem = NULL;

  free(peer_dsc_mem);
  peer_dsc_mem = NULL;
}

/****************************************************************************
 * Public Functions
 ****************************************************************************/

int peer_init(int max_peers, int max_svcs, int max_chrs, int max_dscs)
{
  int rc;

  /* Free memory first in case this function gets called more than once. */

  peer_free_mem();

  peer_mem = malloc(OS_MEMPOOL_BYTES(max_peers, sizeof(struct peer)));
  if (peer_mem == NULL)
    {
      rc = BLE_HS_ENOMEM;
      goto err;
    }

  rc = os_mempool_init(&peer_pool, max_peers, sizeof(struct peer), peer_mem,
                       "peer_pool");
  if (rc != 0)
    {
      rc = BLE_HS_EOS;
      goto err;
    }

  peer_svc_mem = malloc(OS_MEMPOOL_BYTES(max_svcs, sizeof(struct peer_svc)));
  if (peer_svc_mem == NULL)
    {
      rc = BLE_HS_ENOMEM;
      goto err;
    }

  rc = os_mempool_init(&peer_svc_pool, max_svcs, sizeof(struct peer_svc),
                       peer_svc_mem, "peer_svc_pool");
  if (rc != 0)
    {
      rc = BLE_HS_EOS;
      goto err;
    }

  peer_chr_mem = malloc(OS_MEMPOOL_BYTES(max_chrs, sizeof(struct peer_chr)));
  if (peer_chr_mem == NULL)
    {
      rc = BLE_HS_ENOMEM;
      goto err;
    }

  rc = os_mempool_init(&peer_chr_pool, max_chrs, sizeof(struct peer_chr),
                       peer_chr_mem, "peer_chr_pool");
  if (rc != 0)
    {
      rc = BLE_HS_EOS;
      goto err;
    }

  peer_dsc_mem = malloc(OS_MEMPOOL_BYTES(max_dscs, sizeof(struct peer_dsc)));
  if (peer_dsc_mem == NULL)
    {
      rc = BLE_HS_ENOMEM;
      goto err;
    }

  rc = os_mempool_init(&peer_dsc_pool, max_dscs, sizeof(struct peer_dsc),
                       peer_dsc_mem, "peer_dsc_pool");
  if (rc != 0)
    {
      rc = BLE_HS_EOS;
      goto err;
    }

  return 0;

err:
  peer_free_mem();
  return rc;
}
